% Global asymptotic fault-tolerant tracking for time-varying nonlinear
% complex systems with prescribed performance
params=struct();
params.M=@(t)(1.2+0.1*sin(2*t));
params.K=@(t)(8+2*sin(2*pi*t));
params.C=@(t)(2+0.2*cos(0.5*t));
params.d1=@(t)(0.2*sin(0.5*t));
params.d2=@(t)(0.5*cos(1.2*t));
params.kappa=@(t)(0.7+0.2*sin(t));
params.ubar=@(t)(1.2*sin(2*t)+0.5);
params.yd=@(t)(0.5*sin(1.5*t));
params.T=1.5;
params.Ts=1.5;
params.eps=0.1;
params.rho=0.8;
params.k1=10;
params.k2=20;
params.gamma1=0.01;
params.gamma2=0.001;
params.sigma1=@(t)(5*exp(-0.01*t));
params.sigma2=@(t)(5*exp(-0.01*t));
params.sigmarho1=@(t)(0.1*exp(-0.1*t));
params.sigmarho2=@(t)(0.1*exp(-0.1*t));
diffs(params);

initial=[-0.3;0;zeros(2,1)];

[d0,s0]=rhs(0,initial,params);

%%
opt=odeset("OutputFcn","odeplot");
% opt=struct("step",0.01);
[tdata,ydata]=ode15s(@(t,x)rhs(t,x,params),[0 20],initial,opt);
signals=repmat(s0,length(tdata),1);
for it=1:length(tdata)
    [di,si]=rhs(tdata(it),ydata(it,:)',params);
    signals(it)=si;
end

%%
figure();
hold on; grid on;
plot(tdata,horzcat(signals.yd),"DisplayName","y_d");
plot(tdata,ydata(:,1),"DisplayName","y");